using System;
using RaytracerLib.MathLib;
using YAXLib;
using System.Diagnostics;
using TomanuExtensions;
using RaytracerLib.Rays;
using System.Linq;
using System.Runtime.InteropServices;
using RaytracerLib.MathLib.Polynomials;
using RaytracerLib.MathLib.RootFinders;
using RaytracerLib.UVMappers;
using System.Numerics;

namespace RaytracerLib.RenderableObjects
{
    public class SuperquadricObject : RenderableObject
    {
        private RootFinder m_root_finder;

        [YAXNode]
        public Vector3 Exponents = new Vector3(1, 1, 1);

        public SuperquadricObject(Vector3 a_right, Vector3 a_up) :
            base(a_right, a_up)
        {
            Name = "Superquadric";
            Closed = true;
            m_uv_mapper = new SphericalUVMapper();
            Update(UpdateFlags.All);
        }

        private double Norm(Vector3 a_point)
        {
            return Math.Pow(Math.Abs(a_point.X), Exponents.X) +
                    Math.Pow(Math.Abs(a_point.Y), Exponents.Y) +
                    Math.Pow(Math.Abs(a_point.Z), Exponents.Z);
        }

        private Vector3 Gradient(Vector3 a_point)
        {
            return new Vector3(
                Exponents.X * Math.Sign(a_point.X) * Math.Pow(Math.Abs(a_point.X), Exponents.X - 1),
                Exponents.Y * Math.Sign(a_point.Y) * Math.Pow(Math.Abs(a_point.Y), Exponents.Y - 1),
                Exponents.Z * Math.Sign(a_point.Z) * Math.Pow(Math.Abs(a_point.Z), Exponents.Z - 1));
        }

        private double GetDistToNearestAxisOrOneOneBoxPlaneAndInsideOneOneBox(
            Vector3 a_ray_start, Vector3 a_ray_dir)
        {
            double dist_min = Double.PositiveInfinity;

            double dist = -(a_ray_start.Z / a_ray_dir.Z);
            if (dist > 0)
            {
                Vector3 p = (a_ray_start + a_ray_dir * dist);
                if (p.Abs() <= Vector3.ONE)
                    dist_min = dist;
            }

            dist = -(a_ray_start.X / a_ray_dir.X);
            if ((dist > 0) && (dist < dist_min))
            {
                Vector3 p = (a_ray_start + a_ray_dir * dist);
                if (p.Abs() <= Vector3.ONE)
                    dist_min = dist;
            }

            dist = -(a_ray_start.Y / a_ray_dir.Y);
            if ((dist > 0) && (dist < dist_min))
            {
                Vector3 p = (a_ray_start + a_ray_dir * dist);
                if (p.Abs() <= Vector3.ONE)
                    dist_min = dist;
            }

            dist = Plane.GetDist(a_ray_start, a_ray_dir, Vector3.XAXIS, Vector3.XAXIS);
            if ((dist > 0) && (dist < dist_min))
            {
                Vector3 p = (a_ray_start + a_ray_dir * dist);
                if (p.Abs().IsAlmostLessThen(Vector3.ONE))
                    dist_min = dist;
            }

            dist = Plane.GetDist(a_ray_start, a_ray_dir, Vector3.XAXIS, -Vector3.XAXIS);
            if ((dist > 0) && (dist < dist_min))
            {
                Vector3 p = (a_ray_start + a_ray_dir * dist);
                if (p.Abs().IsAlmostLessThen(Vector3.ONE))
                    dist_min = dist;
            }

            dist = Plane.GetDist(a_ray_start, a_ray_dir, Vector3.YAXIS, Vector3.YAXIS);
            if ((dist > 0) && (dist < dist_min))
            {
                Vector3 p = (a_ray_start + a_ray_dir * dist);
                if (p.Abs().IsAlmostLessThen(Vector3.ONE))
                    dist_min = dist;
            }

            dist = Plane.GetDist(a_ray_start, a_ray_dir, Vector3.YAXIS, -Vector3.YAXIS);
            if ((dist > 0) && (dist < dist_min))
            {
                Vector3 p = (a_ray_start + a_ray_dir * dist);
                if (p.Abs().IsAlmostLessThen(Vector3.ONE))
                    dist_min = dist;
            }

            dist = Plane.GetDist(a_ray_start, a_ray_dir, Vector3.ZAXIS, Vector3.ZAXIS);
            if ((dist > 0) && (dist < dist_min))
            {
                Vector3 p = (a_ray_start + a_ray_dir * dist);
                if (p.Abs().IsAlmostLessThen(Vector3.ONE))
                    dist_min = dist;
            }

            dist = Plane.GetDist(a_ray_start, a_ray_dir, Vector3.ZAXIS, -Vector3.ZAXIS);
            if ((dist > 0) && (dist < dist_min))
            {
                Vector3 p = (a_ray_start + a_ray_dir * dist);
                if (p.Abs().IsAlmostLessThen(Vector3.ONE))
                    dist_min = dist;
            }

            return dist_min;
        }

        private double GetMinimalDistanceFromRayToSurface(Vector3 a_ray_start, Vector3 a_ray_dir,
            double a_a, double a_b)
        {
            bool start_inside = Inside(a_ray_start);
            bool inside = start_inside;
            double min_t = Double.PositiveInfinity;

            for (int i = 0; i < Constants.ROOT_FINDER_MAXIMUM_ITERATIONS; i++)
            {
                if (Math.Abs(a_a - a_b) < Constants.ROOT_FINDER_ABSOLUTE_ERROR)
                    break;

                min_t = (a_a + a_b) / 2;
                Vector3 point = a_ray_start + a_ray_dir * min_t;
                Vector3 mid_grad = Gradient(point);

                // Norm(point)
                double norm = (mid_grad * (point / Exponents));

                if (start_inside)
                {
                    if (norm > 1)
                    {
                        inside = false;
                        break;
                    }
                }
                else
                {
                    if (norm < 1)
                    {
                        inside = true;
                        break;
                    }
                }

                double mid_dot = mid_grad * a_ray_dir;

                if (mid_dot < 0)
                    a_a = min_t;
                else
                    a_b = min_t;
            }

            if (inside == start_inside)
                return Double.PositiveInfinity;

            return min_t;
        }

        private bool Inside(Vector3 a_local_point)
        {
            return !Norm(a_local_point).IsAlmostGreaterThen(1);
        }

        internal override Intersection GetIntersection(Intersection a_source_ray_intersection, Ray a_ray)
        {
            Vector3 local_dir;
            Vector3 local_start;
            double near_t;
            double far_t;

            TransformToLocal(a_ray, out local_dir, out local_start);

            if (!GetLocalBoundBox().GetIntersections(local_start, local_dir, out near_t, out far_t))
                return Scene.NoIntersection;

            Func<double, double> f = (t) => Norm(local_start + local_dir * t) - 1;

            bool back_hit = false;

            if (near_t < 0)
            {
                if (Inside(local_start))
                    back_hit = true;
                if (Norm(local_start).IsAlmostEquals(1))
                {
                    Vector3 normal = -Gradient(local_start).Normalized;
                    if (normal * local_dir > 0)
                        back_hit = true;

                    local_start = local_start + local_dir * Constants.MINIMAL_DISTANT;
                    far_t -= Constants.MINIMAL_DISTANT;
                    near_t -= Constants.MINIMAL_DISTANT;
                }
            }

            if (back_hit && OneSide)
                return Scene.NoIntersection;

            if (near_t > 0)
            {
                local_start = local_start + local_dir * (near_t - Constants.MINIMAL_DISTANT * 2);
                far_t = far_t - (near_t - Constants.MINIMAL_DISTANT * 2);
                near_t = Constants.MINIMAL_DISTANT * 2;
            }

            double local_dist = Double.PositiveInfinity;

            for (; ; )
            {
                double plane_t = GetDistToNearestAxisOrOneOneBoxPlaneAndInsideOneOneBox(
                    local_start, local_dir);

                if (plane_t == Double.PositiveInfinity)
                    break;

                double min_t = GetMinimalDistanceFromRayToSurface(local_start, local_dir, 0, plane_t);

                if (min_t != Double.PositiveInfinity)
                {
                    local_dist = m_root_finder.FindRoot(0, min_t, f);

                    if (local_dist == Double.PositiveInfinity)
                        local_dist = m_root_finder.FindRoot(min_t, plane_t, f);
                }
                else
                    local_dist = m_root_finder.FindRoot(0, plane_t, f);

                if (local_dist != Double.PositiveInfinity)
                    break;

                local_start = local_start + local_dir * plane_t;
                far_t -= plane_t;
            }

            if (local_dist == Double.PositiveInfinity)
                return Scene.NoIntersection;

            Vector3 local_pos = local_start + local_dir * local_dist;
            Vector3 world_pos = LocalToWorld * local_pos;
            double world_dist = (world_pos - a_ray.Start).Length;

            Vector3 local_norm = -Gradient(local_pos).Normalized;

            Intersection intersection = new Intersection()
            {
                PrevIntersection = a_source_ray_intersection,
                SceneObject = this,
                SourceRay = a_ray,
                LocalPos = local_pos,
                Dist = world_dist,
                Scene = Scene,
                BackHit = back_hit,
                Pos = world_pos
            };

            // Numerical errors.
            if (intersection.Normal * intersection.SourceRay.Dir >= 0)
                return Scene.NoIntersection;

            return intersection;
        }

        protected override AABB GetLocalBoundBox()
        {
            return new AABB(-Vector3.ONE, Vector3.ONE);
        }

        internal override Vector3 GetNormal(Intersection a_intersection)
        {
            if (a_intersection.BackHit)
                return (LocalToWorldNormal * -Gradient(a_intersection.LocalPos)).Normalized;
            else
                return (LocalToWorldNormal * Gradient(a_intersection.LocalPos)).Normalized;
        }

        internal override Vector3 GetUVW(Intersection a_intersection)
        {
            return base.GetUVW(a_intersection) * 0.5 + Vector3.HALF;
        }

        public override string ToString()
        {
            return String.Format("Superquadric: {0}", Name, Exponents);
        }

        internal override void GetTangents(Intersection a_intersection, out Vector3 a_tangent_x,
            out Vector3 a_tangent_y)
        {
            a_tangent_x = Vector3.CrossProduct(Up, a_intersection.Pos).Normalized;

            if (a_intersection.BackHit)
                a_tangent_y = Vector3.CrossProduct(a_intersection.Normal, a_tangent_x).Normalized;
            else
                a_tangent_y = Vector3.CrossProduct(a_tangent_x, a_intersection.Normal).Normalized;
        }

        protected override void RenderStart(RenderStartPhase a_phase)
        {
            base.RenderStart(a_phase);

            if (a_phase == RenderStartPhase.PrepareObjectToRender)
                m_root_finder = RootFinder.Create(Scene.RenderOptions.RootFindingMethod);
        }

        public override void ScaleAbsolute(double a_scale)
        {
            Scale *= a_scale;

            base.ScaleAbsolute(a_scale);
        }
    }
}